/*
lifter.c
offer lifer control function
designed by lunanting

----------------------
2020-04-28 20:15:16
create file
----------------------
*/
#include "lifter.h"
#include "stdlib.h"
#include "bsp.h"
#include "aos/kernel.h"
#include "aos/kv.h"
#include "k_api.h"


#define FAULT_DIS 100.0

static LIFTER_class lifter={0};
static aos_mutex_t lifter_mutex;
static u8 move_quit=0;
static float g_lifter_speed = 0;

int xp_lifter_block_init(void);
void xp_lifter_move_thread(void *arg);

/*
lifter config motor 
*/
void xp_lifter_config(float speed){
    int motor_speed=xp_lifter_get_speed(speed);

    g_lifter_speed = speed;

    //配置限位
    lifter.motor[0].config.home_offset=lifter.motor[1].config.home_offset=0;
    lifter.motor[0].config.max_limit=lifter.motor[1].config.max_limit=40000000;
    lifter.motor[0].config.min_limit=lifter.motor[1].config.min_limit=-400000000;

    //配置电机速度
    lifter.motor[0].config.speed[0]=lifter.motor[1].config.speed[0]=600;
    lifter.motor[0].config.speed[1]=lifter.motor[1].config.speed[1]=motor_speed;

    //配置电机加速度
    lifter.motor[0].config.acc[0]=lifter.motor[1].config.acc[0]=300;
    lifter.motor[0].config.acc[1]=lifter.motor[1].config.acc[1]=300;

    //p配置电机的减速度
    lifter.motor[0].config.dec[0]=lifter.motor[1].config.dec[0]=300;
    lifter.motor[0].config.dec[1]=lifter.motor[1].config.dec[1]=300;
}

int xp_lifter_read_kv()
{
    char v[20] = {0};
    int v_len = 20;
    int ret = -1;
    uint8_t retry_cnt = 5;

    //读取升降初始零位最多尝试5次
    retry_cnt = 5;
    memset(v, 0, sizeof(v));
    while(--retry_cnt)
    {
        ret = aos_kv_get("lifter.origin",v,&v_len);
        if(0 == ret){
            break;
        }
        else{
            println("aos_kv_get lifter.origin ret %d", ret);
        }
    }
    if(strlen(v)>0){
        lifter.origin_encode[0]=atoi(v);
        println("aos_kv_get lifter.origin %d", lifter.origin_encode[0]);
    }
    else{
        lifter.origin_encode[0]=lifter.encode[0];
        println("aos_kv_get lifter.origin failed! use current lifter position as zero!");
    }

    //读取升降上限位最多尝试5次
    retry_cnt = 5;
    memset(v, 0, sizeof(v));
    while(--retry_cnt)
    {
        ret = aos_kv_get("lifter.top_hight",v,&v_len);
        if(0 == ret){
            break;
        }
        else{
            println("aos_kv_get lifter.top_hight ret %d", ret);
        }
    }
    if(strlen(v)>0){
        lifter.top_hight=atof(v);
        println("aos_kv_get lifter.top_hight %f", lifter.top_hight);
    }
    else{
        lifter.top_hight=LIFTER_top_hight;
        println("aos_kv_get lifter.top_hight failed! use default %f", lifter.top_hight);
    }

    //读取升降下限位最多尝试5次
    retry_cnt = 5;
    memset(v, 0, sizeof(v));
    while(--retry_cnt)
    {
        ret = aos_kv_get("lifter.bottom_hight",v,&v_len);
        if(0 == ret){
            break;
        }
        else{
            println("aos_kv_get lifter.bottom_hight ret %d", ret);
        }
    }
    if(strlen(v)>0){
        lifter.bottom_hight=atof(v);
        println("aos_kv_get lifter.bottom_hight %f", lifter.bottom_hight);
    }
    else{
        lifter.bottom_hight=LIFTER_bottom_hight;
        println("aos_kv_get lifter.bottom_hight failed! use default %f", lifter.bottom_hight);
    }

    return 0;
}

/*
    用于不重启恢复时重新配置升降
*/
int xp_lifter_recover(u8 port,int speed,u8 addr){
    int sta = 0;
    
    lifter.motor[0].port=port;
    lifter.motor[0].speed=speed;
    lifter.motor[0].slave_id=addr;

    xp_lifter_config(LIFTER_line_speed);
    sta=xp_pans_init(&lifter.motor[0]);
    if(sta!=0){
        println("error:lifter 0 recover fail,xp_pans_init() return %d",sta);
        return -1;
    }
    // if(xp_lifter_get_encode(0)!=0)return -1;
    // //读取相关配置值
    // xp_lifter_read_kv();
    // aos_msleep(50);
    xp_lifter_get_location();
    if(xp_lifter_block_init()!=0)return -3;

    println("lifter module recover success~");
    return 0;
}

/*
lifter motor init
param:
    line_speed: lifter speed
    port:modbus uart port
    speed:modbus uart speed
return:
    0=success
*/
int xp_lifter_init(u8 port,int speed,u8 addr){
    int sta = 0;
    
    lifter.motor[0].port=port;
    lifter.motor[0].speed=speed;
    lifter.motor[0].slave_id=addr;
#if LIFTER_motor==2
    lifter.motor[1].port=port;
    lifter.motor[1].speed=speed;
    lifter.motor[1].slave_id=addr+1;
#endif

    xp_lifter_config(LIFTER_line_speed);
    sta=xp_pans_init(&lifter.motor[0]);
    if(sta!=0){
        println("error:lifter 0 init fail,xp_pans_init() return %d",sta);
        return -1;
    }
    if(xp_lifter_get_encode(0)!=0)return -1;

    //读取相关配置值
    xp_lifter_read_kv();
    
    aos_msleep(50);
    xp_lifter_get_location();
#if LIFTER_motor==2
    sta=xp_pans_init(&lifter.motor[1]);
    if(sta!=0){
        println("error:lifter 1 init fail,xp_pans_init() return %d",sta);
        return -2;
    }
    xp_lifter_get_encode(1);
    lifter.origin_encode[1]=lifter.encode[1];
#endif
    if(xp_lifter_block_init()!=0)return -3;
    if(aos_mutex_new(&lifter_mutex)!=0)return -4;
    if(aos_task_new("lifter list",xp_lifter_move_thread,NULL,1024)!=0){
        perr("create lifter list task fail~");
        return -5;
    }
    lifter.init=1;
    println("lifter module init success~");
    return 0;
}

/*clear lifter warn
*/
int xp_clear_lifter_warn(void){
    PANS_handler *motor = &lifter.motor[0];
    gpio_dev_t gpio;
	uint32_t lifter_err = 0;
    u8 clear_warn_try_again_cnt;

    //检测是否清成功，不成功重试
    clear_warn_try_again_cnt = 5;
    while(clear_warn_try_again_cnt--)
    {
        xp_io_write_pin(1,LIFTER_io_left_action,LIFTER_io_stop);
        // xp_io_write_pin(1,LIFTER_io_right_action,LIFTER_io_stop);

        aos_msleep(50);
        if(xp_pans_en(motor,pans_off)!=0)
        {
            println("lifter enable off faild!!!!!!!!");
            continue;
        }
        aos_msleep(50);
        if(xp_pans_run(motor,pans_off)!=0)
        {
            println("lifter run stop config faild!!!!!!!!");
            continue;
        }
        aos_msleep(50);
        if(xp_pans_clear_warn(motor)!=0)
        {
            println("lifter warn clear faild!!!!!!!!");
            continue;
        }
        aos_msleep(50);
        if(xp_pans_en(motor,pans_on)!=0)
        {
            println("lifter enable config faild!!!!!!!!");
            continue;
        }
        aos_msleep(50);
        if(xp_pans_run(motor,pans_on)!=0)
        {
            println("lifter run config faild!!!!!!!!");
            continue;
        }

        //升降驱动器错误检测
        gpio.port = 107;
        hal_gpio_input_get(&gpio, &lifter_err);
        if(1 == lifter_err)
        {
            aos_msleep(200);
        }
        else
        {
            lifter.error = 0;
            xp_lifter_get_location();//更新机构的location
            println("lifter warn clear success! try cnt = %d",(5-clear_warn_try_again_cnt));
            xp_io_write_pin(1,12,0);                //松开升降左侧抱闸
            // xp_io_write_pin(1,13,0);             //松开升降右侧抱闸
            return 0;
        }
    }

    println("lifter warn clear faild!!!");
    return -1;
}

/*
lifter block control init
return:
    0=success
*/
int xp_lifter_block_init(void){
    if(xp_pans_config_move(&lifter.motor[0],10,2,0)!=0)return -1;
    if(xp_pans_compare_block(&lifter.motor[0],1,7,3,524288)!=0)return -2;
    if(xp_pans_jump_block(&lifter.motor[0],2,1)!=0)return -3;
    if(xp_pans_stop_block(&lifter.motor[0],3,0)!=0)return -4;
    if(xp_pans_compare_block(&lifter.motor[0],4,7,0,524296)!=0)return -5;
    if(xp_pans_jump_block(&lifter.motor[0],5,4)!=0)return -6;
    if(xp_pans_set_block(&lifter.motor[0],3)!=0)return -7;
    xp_io_write_pin(1,LIFTER_io_left_action,LIFTER_io_stop);
    if(xp_pans_run(&lifter.motor[0],pans_on)!=0)return -5;
    #if LIFTER_motor==2
    if(xp_pans_config_move(&lifter.motor[1],10,2,0)!=0)return -1;
    if(xp_pans_compare_block(&lifter.motor[1],1,7,3,524288)!=0)return -2;
    if(xp_pans_jump_block(&lifter.motor[1],2,1)!=0)return -3;
    if(xp_pans_stop_block(&lifter.motor[1],3,0)!=0)return -4;
    if(xp_pans_compare_block(&lifter.motor[1],4,7,0,524296)!=0)return -5;
    if(xp_pans_jump_block(&lifter.motor[1],5,4)!=0)return -6;
    if(xp_pans_set_block(&lifter.motor[1],3)!=0)return -7;
    xp_io_write_pin(1,LIFTER_io_right_action,LIFTER_io_stop);
    if(xp_pans_run(&lifter.motor[1],pans_on)!=0)return -5;
    #endif
    return 0;
}




/*
lifter start
param:
    sta:0=stop,1=start
return:
    0=success
*/
int xp_lifter_start(u8 sta){
    if(sta==0){
        if(xp_pans_slow_stop(&lifter.motor[0],pans_on)!=0){
            xp_io_write_pin(1,LIFTER_io_power_off,0);
            return -1;
        }
#if LIFTER_motor==2
        if(xp_pans_slow_stop(&lifter.motor[1],pans_on)!=0){
            xp_io_write_pin(1,LIFTER_io_power_off,0);
            return -2;
        }
#endif
    }
    else if(sta==1){
        if(xp_pans_run(&lifter.motor[0],pans_on)!=0){
            xp_io_write_pin(1,LIFTER_io_power_off,0);
            return -3;
        }
#if LIFTER_motor==2
        if(xp_pans_run(&lifter.motor[1],pans_on)!=0){
            xp_io_write_pin(1,LIFTER_io_power_off,0);
            return -4;
        }
#endif
    }
    else{
        return 1;
    }
    return 0;
}

/*
change lifter speed
param:
    line_speed:lifter speed,m/s
return: 0-success, other-fail
*/
int xp_lifter_change_speed(float line_speed)
{
    int ret = -1;
    int motor_speed_rpm = 0;

    g_lifter_speed = line_speed;

    motor_speed_rpm = xp_lifter_get_speed(line_speed);
    ret = xp_pans_change_speed(&lifter.motor[0], 1, motor_speed_rpm);
    if(ret != 0)
    {
        println("xp_pans_change_speed %d failed! ret %d", motor_speed_rpm, ret);
        return ret;
    }
    return 0;
}

/*
lifter move support retry
param:
    angle: move angle
    speed_block:speed block
    retry_cnt：want retry num
return:
    0=success
*/
int xp_lifter_move_support_retry(float dis,u8 speed_block,u8 retry_cnt){
    int ret = -1;
    u8 retry_num = 0;

    retry_num = retry_cnt;
    ret = xp_lifter_move(dis,speed_block);
    while(retry_num--){
        if(ret != 0){
            aos_msleep(200);
            println("lifter move failed, retry num %d", retry_num);
            ret = xp_lifter_move(dis,speed_block);
        }
        else{
            return ret;
        }
    }
    if(ret != 0){
        println("lifter move failed, retry cnt %d", retry_cnt);
    }
    return ret;
}

/*
lifter move a distance
param:
    dis:distance
return:
    0=success
*/
int xp_lifter_move(float dis,u8 speed_block){
    int sta=0;
    int wait_time=0;
    int pulse,dir;
    int rec1=0,rec2=0;
    float H=0.0f;
    PANS_encode encode={0};
    u8 stop_flag=0;
    u8 count=0;
    u64 time_r=0;
    u32 need_time;
    u8 abnomal=0;
    int ret = -1;

    if(lifter.init!=1)return -12;
    // if(lifter.error==1)return -1;        //这里有错误不返回，下面执行前会清报警，重试仍异常会返回其它错误代码
    if(aos_mutex_lock(&lifter_mutex,100000)!=0){
        return 1;
    }
    lifter.run=1;

	//限位保护
	if (dis < lifter.bottom_hight){
		println("xp_lifter_move: dis %.3f bottom_hight %.3f", dis, lifter.bottom_hight);
		dis = lifter.bottom_hight;
	}else if(dis > lifter.top_hight){
		dis = lifter.top_hight;
	}
	
    lifter.dis=dis;
    move_quit=0;
    wait_time=lifter.motor[0].config.acc[lifter.speed_block[0]]*
                lifter.motor[0].config.speed[lifter.speed_block[0]]/3000;
    lifter.speed_block[0]=lifter.speed_block[1]=speed_block;
    H=dis-lifter.location;
    // println("dis=%0.3f location=%0.3f",dis,lifter.location);
	if(-0.001<H && H<0.001){
        println("lifter move dis<0.001 mm~,dis=%0.3f,location=%0.3f",dis,lifter.location);
        goto END;
    }
    if(dis>lifter.location){
        dir=1;
        if(xp_io_read_input_pin(1,LIFTER_io_top)==0){
            println("lifter checked top io touched~");
            goto END;
        }
    }
    else if(dis<lifter.location){
        dir=-1;
        if(xp_io_read_input_pin(1,LIFTER_io_bottom)==0){
            println("lifter checked bottom io touched~");
            goto END;
        }
    }
    need_time=(abs((int)(H*1000))/g_lifter_speed)+1000;
    pulse=xp_lifter_get_pulse(H);
#if defined JH
    pulse=-1*pulse;
#endif
    if(xp_lifter_check_warn()!=0){
        ret = xp_clear_lifter_warn();
        if(ret != 0){
            sta= -12;goto END;
        }
        aos_msleep(100);
    }
    println("lifter move over %.3f m,on %d pulse",dis,pulse);

    if(xp_pans_config_move(&lifter.motor[0],speed_block,2,pulse)!=0){sta=-8;goto END;}
#if LIFTER_motor==2
    if(xp_pans_config_move(&lifter.motor[1],speed_block,2,pulse)!=0){sta=-9;goto END;}
#endif
    xp_io_write_pin(1,LIFTER_io_left_action,LIFTER_io_run);
#if LIFTER_motor==2
	xp_io_write_pin(1,LIFTER_io_right_action,LIFTER_io_run);
#endif
    lifter.location=dis;
    time_r=aos_now_ms();
    // if(xp_lifter_start(1)!=0){sta=-10;goto END;} 
    aos_msleep(500);
    while (true)
    {
        if(xp_lifter_check_warn()!=0){
            lifter.error=1;
            // xp_lifter_stop();
            abnomal=1;
            sta=-21;
            break;
        }
#if LIFTER_motor==2
        if(xp_pans_keep_synic(&lifter.motor[0],&lifter.motor[1],10000*10)!=0){
			println("lifter synic fail~");
			sta = -20;
			break;
        }
#endif
        if(dir==-1){
            if(xp_io_read_input_pin(1,LIFTER_io_bottom)==0){
                println("lifter checked bottom io touched~");
                // xp_lifter_stop();
                abnomal=1;
                break;
            }
        }
        else if(dir==1){
#ifdef DRYER
            if(xp_io_read_input_pin(1,LIFTER_io_top)==0){
                println("lifter checked top io touched~");
                // xp_lifter_stop();
                abnomal=1;
                break;
            }
#endif
        }
        count++;
        if((aos_now_ms() - time_r) > need_time){
			println("Exceed need_time %d, time_r %lld time_now %lld", need_time, time_r, aos_now_ms());
			abnomal=1;
            break;
        }
        if(move_quit==1){
            abnomal=1;
			println("lifter move quit~");
			break;
		}
        aos_msleep(50);
    }

END:
    lifter.run=0;
    xp_lifter_stop();
    if(abnomal!=0){
        xp_lifter_get_location();
        if(lifter.location<(dis-0.001)||lifter.location>(dis+0.001)){
            println("lifter encode location (%.3f) != dis (%0.3f) , try read again !",lifter.location,dis);
            xp_lifter_get_location();
        }
    }
    //lifter.location+=dis;
    aos_mutex_unlock(&lifter_mutex);
    if(sta!=0){
        println("lifter %d move fail ~ .",sta);
    }
    return sta;
}

/*
single motor move
*/
int xp_lifter_single_move(u8 motor,float dis,u8 speed_block){
    int pulse=0;
    int need_time=(abs((int)(dis*1000))/g_lifter_speed)+1000;
    int pin;
    pulse=xp_lifter_get_pulse(dis);
    if(xp_pans_config_move(&lifter.motor[motor],speed_block,2,pulse)!=0)return -1;
    if(motor==0){
        pin=LIFTER_io_left_action;
    }
    else if(motor==1){
        pin=LIFTER_io_right_action;
    }
    xp_io_write_pin(1,pin,LIFTER_io_run);
    aos_msleep(need_time);
    xp_io_write_pin(1,pin,LIFTER_io_stop);
    return 0;
}

/**************************************非阻塞升降动作控制*******************************/
#define LIFTER_BUF_size    20
static float move_dis[LIFTER_BUF_size];
static int foot_set=0,foot_get=0,move_init=0;
static aos_sem_t move_sem;
static aos_mutex_t set_mux;

/*
set move task
param:
    dis:move distance m
return:
    0=success
    -1= move task init fail~
*/
int xp_lifter_move_set(float dis){

    if(move_init==0){
        perr("lifter list task not inited!");
        return -1;
    }
    aos_mutex_lock(&set_mux,AOS_WAIT_FOREVER);
    move_dis[foot_set++]=dis;
    if(foot_set==LIFTER_BUF_size){
        foot_set=0;
    }
    aos_sem_signal(&move_sem);
    aos_mutex_unlock(&set_mux);
    return 0;
}


/*
get list task move distance
return:move distance
*/
float xp_lifter_move_get(void){
    float ret;
    if(move_init==0){
        perr("lifter list task not inited!");
        return FAULT_DIS;
    }
    ret=move_dis[foot_get++];
    if(foot_get==LIFTER_BUF_size){
        foot_get=0;
    }
    return ret;
}


/*
clear move task
*/
void xp_lifter_move_clear(void){
    
    aos_mutex_lock(&set_mux,AOS_WAIT_FOREVER);
    for(int i=0;i<LIFTER_BUF_size;i++){
        move_dis[i]=FAULT_DIS;
    }
    foot_get=0;
    foot_set=0;
    krhino_sem_count_set(move_sem.hdl,0);
    xp_lifter_stop();
    aos_mutex_unlock(&set_mux);
}


/*
get move status
return: lifter move status
    0=free,1=moving
*/
int xp_lifter_moveStatus_get(void){
    return lifter.run;
}


/*
lifter move task thread
*/
void xp_lifter_move_thread(void *arg){
    float dis=FAULT_DIS;

    if(aos_mutex_new(&set_mux)!=0){
        perr("create set_mux fail~");
        aos_task_exit(-1);
    }
    if(aos_sem_new(&move_sem,0)!=0){
        perr("create move_sem fail~");
        aos_task_exit(-1);
    }
    move_init=1;
    println("lifter list task create success!!");
    while (1){
        aos_sem_wait(&move_sem,AOS_WAIT_FOREVER);
        dis=xp_lifter_move_get();
        if(dis==FAULT_DIS){
            continue;
        }
        println("lifter move to %0.3f",dis);
        if(xp_lifter_move(dis,1)!=0){
            perr("lifter move action fail~");
            continue;
        }
    }
}

/*------------------------------------------------------------------------*/
/*
lifter touch io and slower motor
param:
    slow_time:slow speed run time
return:
    0=success
*/
int xp_lifter_io_touch(u8 pin,u16 slow_time){
    
    switch(pin){
        case LIFTER_io_up:

            break;
    }
    xp_pans_slow_stop(&lifter.motor[0],pans_on);
    #if LIFTER_motor==2
    xp_pans_slow_stop(&lifter.motor[1],pans_on);
    #endif
    xp_pans_slow_stop(&lifter.motor[0],pans_off);
    #if LIFTER_motor==2
    xp_pans_slow_stop(&lifter.motor[1],pans_off);
    #endif

}

/*
lifter check zero
return:
    0=success
*/
int xp_lifter_check_zero(void){
    lifter.check_zero=0;
    if(xp_lifter_get_encode(0)!=0)return -2;
    #if LIFTER_motor==2
    if(xp_lifter_get_encode(1)!=0)return -2;
    #endif
    println("lifter check zero start......");
    if(xp_io_read_input_pin(1,LIFTER_io_origin)!=LIFTER_io_touch){

        xp_pans_fast_stop(&lifter.motor[0],pans_off);
        if(xp_lifter_move(-2,1)!=0)return -3;
        if(xp_io_read_input_pin(1,LIFTER_io_down)==LIFTER_io_touch){
            if(xp_lifter_move(-0.05,0)!=0)return -4;
        }
        if(xp_lifter_move(-2,1)!=0)return -5;
        if(xp_io_read_input_pin(1,LIFTER_io_bottom)==LIFTER_io_touch){
            if(xp_io_read_input_pin(1,LIFTER_io_origin)!=LIFTER_io_touch){
                if(xp_lifter_move(1,1)!=0)return -6;
            }
        }
        if(xp_io_read_input_pin(1,LIFTER_io_origin)==LIFTER_io_touch){
            lifter.check_zero=1;
            lifter.location=0;
        }
        else{
            return -4;
        }
    }
    if(xp_lifter_get_encode(0)!=0)return -3;
    #if LIFTER_motor==2
    if(xp_lifter_get_encode(1)!=0)return -3;
    #endif
    lifter.origin_encode[0]=lifter.encode[0];
    return 0;
}


/*
get lifter motor encode
return:
    0=success
*/
int xp_lifter_get_encode(u8 motor){
    PANS_encode encode={0};

    if(xp_pans_get_encode(&lifter.motor[motor],&encode)!=0){
        println("error:get lifter encode fail~");
        return -1;
    }
    //println("lifter %d encode=%d.%d, h = %llu",motor,encode.r,encode.p,encode.h);
    lifter.encode[motor]=encode.h;
    return 0;
}

/*
get lifter motor speed
param:
    line_speed:lifter line speed,m/s
return:motor speed,r/min
*/
int xp_lifter_get_speed(float line_speed){
    float motor_speed;
    
    motor_speed=line_speed*60.0*LIFTER_ratio/LIFTER_pitch;
    return (int)motor_speed;
}

/*
get lifter motor pulse number
param:
    dis:lifter move distance,m
return:motor pulse
*/
int xp_lifter_get_pulse(float dis){
    double pulse=0;

    pulse=dis*LIFTER_part*LIFTER_ratio/LIFTER_pitch;
    return (int)pulse;
}


/*
convert encode to location
将编码器值换算成位置
return:location
*/
float xp_lifter_get_location(void){
    int d=0;

    if(xp_lifter_get_encode(0)!=0)return -1;
    // if(xp_lifter_get_encode(1)!=0)return -1;
    d=lifter.encode[0]-lifter.origin_encode[0];
#ifdef JH
    lifter.location=((-1*d/838.8608)/10000)*LIFTER_pitch/LIFTER_ratio;
#else
    lifter.location=((d/838.8608)/10000)*LIFTER_pitch/LIFTER_ratio;
#endif
    //println("lifter location = %0.3f",lifter.location);
    return lifter.location;
}



/*
check lifter motor warnning,and repair
return:
    0=success
*/
int xp_lifter_check_warn(void){
    u8 count=LIFTER_motor;
    for(u8 i=0;i<count;i++){
        if(xp_pans_get_warn_state(&lifter.motor[i])!=pans_off){
            perr("lifter %d motor have a error !!",i);
            if(xp_pans_get_error_code(&lifter.motor)==0){
                perr("485 connect not normal!!!!  need check why~~~~");
                return 0;
            }
            return -1;
            // if(xp_pans_clear_warn(&lifter.motor[i])!=0)return -2;
            // if(xp_pans_en(&lifter.motor[i],pans_on)!=0)return -1;
        }
    }
    return 0;
}

/*
lifter run over distance
param:
    dis:distance
return:
    0=success
*/
int xp_lifter_run_over(float dis){
    if(dis>0){  //前进
        if(xp_io_read_input_pin(1,LIFTER_io_top)==LIFTER_io_touch){
            println("wanrn:lifter already on the top,can't move up any more!");
            return 1;
        }
            
    }
    else if(dis==0){    //停止

    }
    else{   //后退

    }
    return 0;
}


/*
lifter run to a location
param:
    location:location value
return:
    0=success
*/
int xp_lifter_run_to(float location){
    
}


/*
lifter stop
*/
int xp_lifter_stop(void){
    // rec1=xp_pans_slow_stop(&lifter.motor[0],pans_on);
    // rec2=xp_pans_slow_stop(&lifter.motor[1],pans_on);
    xp_io_write_pin(1,LIFTER_io_left_action,LIFTER_io_stop);
#if LIFTER_motor==2
    xp_io_write_pin(1,LIFTER_io_right_action,LIFTER_io_stop);
#endif
    //println("lifter stop!!");
    move_quit=1;
#ifndef DRYER
    extern void xp_clear_lifter_signal();
    xp_clear_lifter_signal();
#endif
    return 0;
}

/*
set as zero point
return:
    0=success
*/
int xp_lifter_as_zero(void){
    char v[20];

    if(xp_lifter_get_encode(0)!=0){
		println("xp_lifter_get_encode failed!");
		return -1;
	}
    lifter.origin_encode[0]=lifter.encode[0];
    lifter.location=0;
    sprintf(v,"%d",lifter.origin_encode[0]);
    if(aos_kv_set("lifter.origin",v,strlen(v),1)!=0)return -2;
    println("lifter.origin set as: %s",v);
}

/*
clear zero recode
*/
void xp_lifter_zero_clear(void){
    aos_kv_del("lifter.origin");
}

/*
set top hight
return:
    0=success
*/
int xp_lifter_set_top(char *str){
    char v[20];

    println("str %s", str);
    strncpy(v, str, sizeof(v)-1);
    if(aos_kv_set("lifter.top_hight",v,strlen(v),1)!=0)return -1;
    println("lifter.top_hight set as: %s",v);
}

/*
set bottom hight
return:
    0=success
*/
int xp_lifter_set_bottom(char *str){
    char v[20];

    println("str %s", str);
    strncpy(v, str, sizeof(v)-1);
    if(aos_kv_set("lifter.bottom_hight",v,strlen(v),1)!=0)return -1;
    println("lifter.bottom_hight set as: %s",v);
}


/*
get lifter location
return: location
*/
float xp_lifter_location_get(void){
    return lifter.location;
}

/*
get lifter top hight
return: top hight
*/
float xp_lifter_get_top_hight(void){
    return lifter.top_hight;
}

/*
lifter relative move
相对移动
param: relative value ; float
return: 
    0=success
*/
int xp_lifter_relative_move(float value){
    return xp_lifter_move(lifter.location+value,1);
}

/*
lifter check io event
param:
    event:event value
*/
void xp_lifter_io_event(u32 event){
    static u8 bottom=1,origin=1,up=1,down=1,top=1;
    char tips[50]={0};
    u8 state,stop_flag=0;
    println("check lifter io touch!");
    state=(event>>LIFTER_io_up)&0x000001;
    if(state!=up){
        up=state;
        if(up==0&&lifter.dis>0&&lifter.run==1){
            stop_flag=1;
            sprintf(tips,"touch up sensor");
        }
    }
    state=(event>>LIFTER_io_down)&0x000001;
    if(state!=down){
        down=state;
        if(down==0&&lifter.dis<0&&lifter.run==1){
            stop_flag=1;
            sprintf(tips,"touch down sensor");
        }
    }
    state=(event>>LIFTER_io_origin)&0x01;
    if(state!=origin){
        origin=state;
        if(origin==0&&lifter.check_zero==0){
            stop_flag=1;
            sprintf(tips,"on the origin point");
        }
    }
    state=(event>>LIFTER_io_bottom)&0x01;
    if(bottom!=state){
        bottom=state;
        if(bottom==0&&lifter.run==1&&lifter.dis<0){
            stop_flag=1;
            sprintf(tips,"on the bottom point");
        }
    }
    state=(event>>LIFTER_io_top)&0x01;
    if(top!=state){
        top=state;
        if(top==0&&lifter.run==1&&lifter.dis>0){
            stop_flag=1;
            sprintf(tips,"on the top point");
        }
    }
    if(stop_flag==1){
        stop_flag=0;
        move_quit=1;
        xp_lifter_stop();
        aos_msleep(500);
        println("lifter %s,stop motor!",tips);
    }
}


/****************************lifter debug**********************/
/*
lifter debug
*/
int xp_lifter_debug(char *type,char *fun,char *param){
    float dis = 0;
    float top_hight = 0;
    float bottom_hight = 0;

    if(strcmp(type,"lifter")!=0)return 0;
    if(strcmp(fun,"move")==0){
        dis=(float)atof(param);
        xp_lifter_move(dis,1);
    }
    else if(strcmp(fun,"support_retry")==0){
        dis=(float)atof(param);
        xp_lifter_move_support_retry(dis,1,3);
    }
    else if(strcmp(fun,"stop")==0){
        xp_lifter_stop();
    }
    else if(strcmp(fun,"move0")==0){
        dis=(float)atof(param);
        xp_lifter_single_move(0,dis,1);
    }
    else if(strcmp(fun,"move1")==0){
        dis=(float)atof(param);
        xp_lifter_single_move(1,dis,1);
    }
    else if(strcmp(fun,"encode")==0){
        xp_lifter_get_encode(0);
        xp_lifter_get_encode(1);
    }
    else if(strcmp(fun,"location")==0){
        println("lifter now location: %0.3f",xp_lifter_get_location());
    }
    else if(strcmp(fun,"check_warn")==0){
        println("warn status: %d",xp_pans_get_warn_state(&lifter.motor[0]));
    }
    else if(strcmp(fun,"get_error")==0){
        xp_pans_get_error_code(&lifter.motor[0]);
    }
    else if(strcmp(fun,"get_encode")==0){
        if(strcmp(param,"0")==0){
            xp_lifter_get_encode(0);
        }
        else if(strcmp(param,"1")==0){
            xp_lifter_get_encode(1);
        }
        else if(strcmp(param,"2")==0){
            xp_lifter_get_encode(0);
            xp_lifter_get_encode(1);
        }
    }
    else if(strcmp(fun,"get_position")==0){
        xp_lifter_get_location();
        println("position: %0.3f m",lifter.location);
    }
    else if(strcmp(fun,"set_zero")==0){
        xp_lifter_as_zero();
    }
    else if(strcmp(fun,"clear_zero")==0){
        xp_lifter_zero_clear();
    }
    else if(strcmp(fun,"set_move")==0){
        xp_lifter_move_set((float)atof(param));
    }
    else if(strcmp(fun,"set_clear")==0){
        xp_lifter_move_clear();
    }
    else if(strcmp(fun,"set_top")==0){
        xp_lifter_set_top(param);
    }
    else if(strcmp(fun,"set_bottom")==0){
        xp_lifter_set_bottom(param);
    }
    else if(strcmp(fun,"clear_warn")==0)
    {
        xp_clear_lifter_warn();
    }
    else{
        return 0;
    }
    return 1;
}


//end of the file
